﻿using Microsoft.Xna.Framework;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using UCS.XNA.Utils.Animation;
using UCS.XNA.Utils.Math;

namespace Hopeless.Physical
{
    public static class Physics
    {
        public static List<OffsetTranslateAnimation> Animations = new List<OffsetTranslateAnimation>();
        public static List<ForceTranslateAnimation> ForceAnimations = new List<ForceTranslateAnimation>();
        public static List<ScaleAnimation> ScaleAnimations = new List<ScaleAnimation>();

        public static bool CheckRotatedRectanglesCollision(Rectangle rectangleA, Vector2 relativeOriginA, float rectangleAAngle, Rectangle rectangleB, Vector2 relativeOriginB, float rectangleBAngle)
        {
            var XULA = rectangleA.X - relativeOriginA.X;
            var YULA = rectangleA.Y - relativeOriginA.Y;
            var XURA = rectangleA.X + rectangleA.Width - relativeOriginA.X;
            var YURA = YULA;
            var XLLA = XULA;
            var YLLA = rectangleA.Y + rectangleA.Height - relativeOriginA.Y;
            var XLRA = XURA;
            var YLRA = YLLA;

            var XULB = rectangleB.X - relativeOriginB.X;
            var YULB = rectangleB.Y - relativeOriginB.Y;
            var XURB = rectangleB.X + rectangleB.Width - relativeOriginB.X;
            var YURB = YULB;
            var XLLB = XULB;
            var YLLB = rectangleB.Y + rectangleB.Height - relativeOriginB.Y;
            var XLRB = XURB;
            var YLRB = YLLB;

            var originA = new Vector2(rectangleA.X, rectangleA.Y);
            var ULA = GeometricAlgebra.RotateVector(new Vector2(XULA, YULA), rectangleAAngle, originA);
            var URA = GeometricAlgebra.RotateVector(new Vector2(XURA, YURA), rectangleAAngle, originA);
            var LLA = GeometricAlgebra.RotateVector(new Vector2(XLLA, YLLA), rectangleAAngle, originA);
            var LRA = GeometricAlgebra.RotateVector(new Vector2(XLRA, YLRA), rectangleAAngle, originA);

            var originB = new Vector2(rectangleB.X, rectangleB.Y);
            var ULB = GeometricAlgebra.RotateVector(new Vector2(XULB, YULB), rectangleBAngle, originB);
            var URB = GeometricAlgebra.RotateVector(new Vector2(XURB, YURB), rectangleBAngle, originB);
            var LLB = GeometricAlgebra.RotateVector(new Vector2(XLLB, YLLB), rectangleBAngle, originB);
            var LRB = GeometricAlgebra.RotateVector(new Vector2(XLRB, YLRB), rectangleBAngle, originB);

            if (!CheckRadiusCollision(rectangleA, ULA, LRA, rectangleB, ULB, LRB))
                return false;

            var axisList = new List<Vector2>();
            axisList.Add(new Vector2(URA.X - ULA.X, URA.Y - ULA.Y));
            axisList.Add(new Vector2(URA.X - LRA.X, URA.Y - LRA.Y));
            axisList.Add(new Vector2(ULB.X - LLB.X, ULB.Y - LLB.Y));
            axisList.Add(new Vector2(ULB.X - URB.X, ULB.Y - URB.Y));

            foreach (var axis in axisList)
            {
                var projectionsA = new List<Vector2>();
                var auxURA = (URA.X * axis.X + URA.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsA.Add(new Vector2(auxURA * axis.X, auxURA * axis.Y));
                var auxULA = (ULA.X * axis.X + ULA.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsA.Add(new Vector2(auxULA * axis.X, auxULA * axis.Y));
                var auxLLA = (LLA.X * axis.X + LLA.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsA.Add(new Vector2(auxLLA * axis.X, auxLLA * axis.Y));
                var auxLRA = (LRA.X * axis.X + LRA.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsA.Add(new Vector2(auxLRA * axis.X, auxLRA * axis.Y));

                var projectionsB = new List<Vector2>();
                var auxURB = (URB.X * axis.X + URB.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsB.Add(new Vector2(auxURB * axis.X, auxURB * axis.Y));
                var auxULB = (ULB.X * axis.X + ULB.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsB.Add(new Vector2(auxULB * axis.X, auxULB * axis.Y));
                var auxLLB = (LLB.X * axis.X + LLB.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsB.Add(new Vector2(auxLLB * axis.X, auxLLB * axis.Y));
                var auxLRB = (LRB.X * axis.X + LRB.Y * axis.Y) / (axis.X * axis.X + axis.Y * axis.Y);
                projectionsB.Add(new Vector2(auxLRB * axis.X, auxLRB * axis.Y));

                float axisLength = axis.Length();
                var dotsA = new List<double>();
                foreach (var item in projectionsA)
                    //dotsA.Add(item.Length() * axisLength * Math.Cos(GeometricAlgebra.GetAngleBetweenVectors(item, axis)));
                    dotsA.Add(item.X * axis.X + item.Y * axis.Y);

                var dotsB = new List<double>();
                foreach (var item in projectionsB)
                    //dotsB.Add(item.Length() * axisLength * Math.Cos(GeometricAlgebra.GetAngleBetweenVectors(item, axis)));
                    dotsB.Add(item.X * axis.X + item.Y * axis.Y);

                var minA = dotsA.Min();
                var maxA = dotsA.Max();
                var minB = dotsB.Min();
                var maxB = dotsB.Max();

                if (minB < minA && maxB < minA)
                    return false;
                if (minA < minB && maxA < minB)
                    return false;
            }

            return true;
        }

        public static bool CheckRadiusCollision(Rectangle rectangleA, Vector2 ULA, Vector2 LRA, Rectangle rectangleB, Vector2 ULB, Vector2 LRB)
        {
            var dirA = ULA - LRA;
            var dirB = ULB - LRB;
            var midA = dirA * 0.5f + LRA;
            var midB = dirB * 0.5f + LRB;
            var diameterA = dirA.Length();
            var diameterB = dirB.Length();

            if ((midA - midB).Length() <= diameterA + diameterB)
                return true;

            return false;
        }

        public static void Update(GameTime gameTime)
        {
            foreach (var animation in Animations)
                animation.Update(gameTime);

            foreach (var animation in ForceAnimations)
                animation.Update(gameTime);

            foreach (var animation in ScaleAnimations)
                animation.Update(gameTime);

            Animations.RemoveAll(a => a.IsFinished);
            ForceAnimations.RemoveAll(a => a.IsFinished);
            ScaleAnimations.RemoveAll(a => a.IsFinished);
        }
    }
}
